/**************************************************************************************************
* Title: SumVector4.cpp
* Author: Gael Huber
* Description: Defines a 4-dimensional vector and associated operations.
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Cross product of two vectors, following the geometric analog of the 3D vector cross product
**************************************************************************************************/
Vector4* Vec4Cross(Vector4* pOut, const Vector4* pV1, const Vector4* pV2, const Vector4* pV3) {
	pOut->x = pV1->y * (pV2->z * pV3->w - pV2->w * pV3->z)
			- pV1->z * (pV2->y * pV3->w - pV2->w * pV3->y)
			+ pV1->w * (pV2->y * pV3->z - pV2->z * pV3->y);

	pOut->y = pV1->x * (pV2->w * pV3->z - pV2->z * pV3->w)
			- pV1->z * (pV2->w * pV3->x - pV2->x * pV3->w)
			+ pV1->w * (pV2->z * pV3->x - pV2->x * pV3->z);

	pOut->z = pV1->x * (pV2->y * pV3->w - pV2->w * pV3->y)
			- pV1->y * (pV2->x * pV3->w - pV2->w * pV3->x)
			+ pV1->w * (pV2->x * pV3->y - pV2->y * pV3->x);

	pOut->w = pV1->x * (pV2->z * pV3->y - pV2->y * pV3->z)
			- pV1->y * (pV2->z * pV3->x - pV2->x * pV3->z)
			+ pV1->z * (pV2->y * pV3->x - pV2->x * pV3->y);	

	return pOut;
}

/**************************************************************************************************
* Normalize the vector
**************************************************************************************************/
Vector4* Vec4Normalize(Vector4* pOut, const Vector4* pV) {
	float l = Vec4Length(pV);
	pOut->x = pV->x / l;
	pOut->y = pV->y / l;
	pOut->z = pV->z / l;
	pOut->w = pV->w / l;
	return pOut;
}

/**************************************************************************************************
* Hermite interpolation between position V1, tangent T1 (when s == 0) and position V2, tangent T2 
* (when s == 1).
**************************************************************************************************/
Vector4* Vec4Hermite(Vector4* pOut, const Vector4* pV1, const Vector4* pT1, const Vector4* pV2, 
	const Vector4* pT2, float s) {
	// Calculate a, b, c, and d values
	float a(2.0f*s*s*s - 3.0f*s*s + 1.0f);
	float b(-2.0f*s*s*s + 3.0f*s*s);
	float c(s*s*s - 2.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for a*v1 + b*v2 + c*t1 + d*t2
	Vector4 v1, v2;

	Vec4Scale(&v1, pV1, a);
	Vec4Scale(&v2, pV2, b);
	Vec4Add(pOut, &v1, &v2);

	Vec4Scale(&v1, pT1, c);
	Vec4Add(pOut, pOut, &v1);

	Vec4Scale(&v1, pT2, d);
	Vec4Add(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* CatmullRom interpolation between V1 (when s == 0) and V2 (when s == 1)
**************************************************************************************************/
Vector4* Vec4CatmullRom(Vector4* pOut, const Vector4* pV0, const Vector4* pV1, const Vector4* pV2, 
	const Vector4* pV3, float s) {
	// Calculate a, b, c, and d
	float a(-s*s*s + 2.0f*s*s - s);
	float b(3.0f*s*s*s - 5.0f*s*s + 2.0f);
	float c(-3.0f*s*s*s + 4.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for (a*p1 + b*p2 + c*p3 + d*p4)/2
	Vector4 v1, v2;

	Vec4Scale(&v1, pV0, a);
	Vec4Scale(&v2, pV1, b);
	Vec4Add(pOut, &v1, &v2);

	Vec4Scale(&v1, pV2, c);
	Vec4Add(pOut, pOut, &v1);

	Vec4Scale(&v1, pV3, d);
	Vec4Add(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* Barycentric coordinates V1 + f(V2 - V1) + g(V3 - V1)
**************************************************************************************************/
Vector4* Vec4Barycentric(Vector4* pOut, const Vector4* pV1, const Vector4* pV2, const Vector4* pV3,
	float f, float g) {
	pOut->x = pV1->x + f * (pV2->x - pV1->x) + g * (pV3->x - pV1->x);
	pOut->y = pV1->y + f * (pV2->y - pV1->y) + g * (pV3->y - pV1->y);
	pOut->z = pV1->z + f * (pV2->z - pV1->z) + g * (pV3->z - pV1->z);
	pOut->w = pV1->w + f * (pV2->w - pV1->w) + g * (pV3->w - pV1->w);
	return pOut;
}


/**************************************************************************************************
* Transform vector by matrix
**************************************************************************************************/
Vector4* Vec4Transform(Vector4* pOut, const Vector4* pV, const Matrix* pM) {
	pOut->x = pM->_11*pV->x + pM->_12*pV->y + pM->_13*pV->z + pM->_14*pV->w;
	pOut->y = pM->_21*pV->x + pM->_22*pV->y + pM->_23*pV->z + pM->_24*pV->w;
	pOut->z = pM->_31*pV->x + pM->_32*pV->y + pM->_33*pV->z + pM->_34*pV->w;
	pOut->w = pM->_41*pV->x + pM->_42*pV->y + pM->_43*pV->z + pM->_44*pV->w;
	return pOut;
}